/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#ifndef __ATTITUDE_H
#define __ATTITUDE_H

#include <stdint.h>
#include "pid.h"
#include "filters.h"
#include "zino.h"

#define XYZ_AXIS_COUNT 3

// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
#define PTERM_SCALE 0.032029f
#define ITERM_SCALE 0.244381f
#define DTERM_SCALE 0.000529f
#define PID_MIXER_SCALING 500
#define PID_SAVED_FLAG 0xDD
// The constant scale factor to replace the Kd component of the feedforward calculation.
// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
#define FEEDFORWARD_SCALE 0.013754f

#ifdef __cplusplus
extern "C"
{
#endif
    typedef uint32_t timeUs_t;
    typedef uint32_t timeDelta_t;

    typedef enum
    {
        PID_ROLL,
        PID_PITCH,
        PID_YAW,
        PID_LEVEL,
        PID_ITEM_COUNT
    } pidIndex_e;

    typedef struct rollAndPitchTrims_s
    {
        int16_t roll;
        int16_t pitch;
    } rollAndPitchTrims_t_def;

    typedef union rollAndPitchTrims_u
    {
        int16_t raw[2];
        rollAndPitchTrims_t_def values;
    } rollAndPitchTrims_t;

    typedef struct pidf_s
    {
        uint8_t P;
        uint8_t I;
        uint8_t D;
        uint16_t F;
    } pidf_t;
    typedef enum
    {
        ANTI_GRAVITY_SMOOTH,
        ANTI_GRAVITY_STEP
    } antiGravityMode_e;
    typedef struct pidCoefficient_s
    {
        float Kp;
        float Ki;
        float Kd;
        float Kf;
    } pidCoefficient_t;
    typedef enum
    {
        PID_CRASH_RECOVERY_OFF = 0,
        PID_CRASH_RECOVERY_ON,
        PID_CRASH_RECOVERY_BEEP,
        PID_CRASH_RECOVERY_DISARM,
    } pidCrashRecovery_e;
    typedef enum
    {
        ITERM_RELAX_GYRO,
        ITERM_RELAX_SETPOINT,
        ITERM_RELAX_TYPE_COUNT,
    } itermRelaxType_e;
    typedef enum
    {
        ITERM_RELAX_OFF,
        ITERM_RELAX_RP,
        ITERM_RELAX_RPY,
        ITERM_RELAX_RP_INC,
        ITERM_RELAX_RPY_INC,
        ITERM_RELAX_COUNT,
    } itermRelax_e;
    typedef enum
    {
        LAUNCH_CONTROL_MODE_NORMAL = 0,
        LAUNCH_CONTROL_MODE_PITCHONLY,
        LAUNCH_CONTROL_MODE_FULL,
        LAUNCH_CONTROL_MODE_COUNT // must be the last element
    } launchControlMode_e;
    typedef enum
    {
        PID_STABILISATION_OFF = 0,
        PID_STABILISATION_ON
    } pidStabilisationState_e;
    typedef enum ffInterpolationType_e
    {
        FF_INTERPOLATE_OFF,
        FF_INTERPOLATE_ON,
        FF_INTERPOLATE_AVG2,
        FF_INTERPOLATE_AVG3,
        FF_INTERPOLATE_AVG4
    } ffInterpolationType_t;
    enum
    {
        AUTO_PROFILE_CELL_COUNT_STAY = 0,    // Stay on this profile irrespective of the detected cell count. Use this profile if no other profile matches (default, i.e. auto profile switching is off)
        AUTO_PROFILE_CELL_COUNT_CHANGE = -1, // Always switch to a profile with matching cell count if there is one
    };

    typedef union dtermLowpass_u
    {
        pt1Filter_t pt1Filter;
        biquadFilter_t biquadFilter;
    } dtermLowpass_t;
#define PID_ITEM_COUNT 4
    typedef struct pidProfile_s
    {
        uint16_t yaw_lowpass_hz;     // Additional yaw filter when yaw axis too noisy
        uint16_t dterm_lowpass_hz;   // Delta Filter in hz
        uint16_t dterm_notch_hz;     // Biquad dterm notch hz
        uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff

        pidf_t pid[PID_ITEM_COUNT];

        uint8_t dterm_filter_type;       // Filter selection for dterm
        uint8_t itermWindupPointPercent; // iterm windup threshold, percent motor saturation
        uint16_t pidSumLimit;
        uint16_t pidSumLimitYaw;
        uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
        uint8_t levelAngleLimit;  // Max angle in degrees in level mode

        uint8_t horizon_tilt_effect;      // inclination factor for Horizon mode
        uint8_t horizon_tilt_expert_mode; // OFF or ON

        // Betaflight PID controller parameters
        uint8_t antiGravityMode;           // type of anti gravity method
        uint16_t itermThrottleThreshold;   // max allowed throttle delta before iterm accelerated in ms
        uint16_t itermAcceleratorGain;     // Iterm Accelerator Gain when itermThrottlethreshold is hit
        uint16_t yawRateAccelLimit;        // yaw accel limiter for deg/sec/ms
        uint16_t rateAccelLimit;           // accel limiter roll/pitch deg/sec/ms
        uint16_t crash_dthreshold;         // dterm crash value
        uint16_t crash_gthreshold;         // gyro crash value
        uint16_t crash_setpoint_threshold; // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
        uint16_t crash_time;               // ms
        uint16_t crash_delay;              // ms
        uint8_t crash_recovery_angle;      // degrees
        uint8_t crash_recovery_rate;       // degree/second
        uint8_t feedForwardTransition;     // Feed forward weight transition
        uint16_t crash_limit_yaw;          // limits yaw errorRate, so crashes don't cause huge throttle increase
        uint16_t itermLimit;
        uint16_t dterm_lowpass2_hz;         // Extra PT1 Filter on D in hz
        uint8_t crash_recovery;             // off, on, on and beeps when it is in crash recovery mode
        uint8_t throttle_boost;             // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
        uint8_t throttle_boost_cutoff;      // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
        uint8_t iterm_rotation;             // rotates iterm to translate world errors to local coordinate system
        uint8_t iterm_relax_type;           // Specifies type of relax algorithm
        uint8_t iterm_relax_cutoff;         // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
        uint8_t iterm_relax;                // Enable iterm suppression during stick input
        uint8_t acro_trainer_angle_limit;   // Acro trainer roll/pitch angle limit in degrees
        uint8_t acro_trainer_debug_axis;    // The axis for which record debugging values are captured 0=roll, 1=pitch
        uint8_t acro_trainer_gain;          // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
        uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot
        uint8_t abs_control_gain;           // How strongly should the absolute accumulated error be corrected for
        uint8_t abs_control_limit;          // Limit to the correction
        uint8_t abs_control_error_limit;    // Limit to the accumulated error
        uint8_t abs_control_cutoff;         // Cutoff frequency for path estimation in abs control
        uint8_t dterm_filter2_type;         // Filter selection for 2nd dterm
        uint16_t dyn_lpf_dterm_min_hz;
        uint16_t dyn_lpf_dterm_max_hz;
        uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
        uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
        uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
        uint8_t launchControlGain;              // Iterm gain used while launch control is active
        uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
        uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
        uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
        uint8_t thrustLinearization;            // Compensation factor for pid linearization
        uint8_t d_min[XYZ_AXIS_COUNT];          // Minimum D value on each axis
        uint8_t d_min_gain;                     // Gain factor for amount of gyro / setpoint activity required to boost D
        uint8_t d_min_advance;                  // Percentage multiplier for setpoint input to boost algorithm
        uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
        int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
        uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
        uint8_t ff_boost;                       // amount of high-pass filtered FF to add to FF, 100 means 100% added
        // char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile

        uint8_t dyn_idle_min_rpm;      // minimum motor speed enforced by the dynamic idle controller
        uint8_t dyn_idle_p_gain;       // P gain during active control of rpm
        uint8_t dyn_idle_i_gain;       // I gain during active control of rpm
        uint8_t dyn_idle_d_gain;       // D gain for corrections around rapid changes in rpm
        uint8_t dyn_idle_max_increase; // limit on maximum possible increase in motor idle drive during active control

        uint8_t ff_interpolate_sp;     // Calculate FF from interpolated setpoint
        uint8_t ff_max_rate_limit;     // Maximum setpoint rate percentage for FF
        uint8_t ff_smooth_factor;      // Amount of smoothing for interpolated FF steps
        uint8_t dyn_lpf_curve_expo;    // set the curve for dynamic dterm lowpass filter
        uint8_t level_race_mode;       // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
        uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
    } pidProfile_t;

    typedef struct pidRuntime_s
    {
        float dT;
        float pidFrequency;
        bool pidStabilisationEnabled;
        float previousPidSetpoint[3];
        filterApplyFnPtr dtermNotchApplyFn;
        biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
        filterApplyFnPtr dtermLowpassApplyFn;
        dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
        filterApplyFnPtr dtermLowpass2ApplyFn;
        dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
        filterApplyFnPtr ptermYawLowpassApplyFn;
        pt1Filter_t ptermYawLowpass;
        bool antiGravityEnabled;
        uint8_t antiGravityMode;
        pt1Filter_t antiGravityThrottleLpf;
        pt1Filter_t antiGravitySmoothLpf;
        float antiGravityOsdCutoff;
        float antiGravityThrottleHpf;
        float antiGravityPBoost;
        // float ffBoostFactor;
        float itermAccelerator;
        uint16_t itermAcceleratorGain;
        float feedForwardTransition;
        pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
        float levelGain;
        float horizonGain;
        float horizonTransition;
        float horizonCutoffDegrees;
        float horizonFactorRatio;
        uint8_t horizonTiltExpertMode;
        float maxVelocity[XYZ_AXIS_COUNT];
        float itermWindupPointInv;
        // bool inCrashRecoveryMode;
        // timeUs_t crashDetectedAtUs;
        timeDelta_t crashTimeLimitUs;
        timeDelta_t crashTimeDelayUs;
        int32_t crashRecoveryAngleDeciDegrees;
        float crashRecoveryRate;
        float crashGyroThreshold;
        float crashDtermThreshold;
        float crashSetpointThreshold;
        float crashLimitYaw;
        float itermLimit;
        bool itermRotation;
        bool zeroThrottleItermReset;
        bool levelRaceMode;

    } pidRuntime_t;
    typedef struct pidAxisData_s
    {
        float P;
        float I;
        float D;
        float F;

        float Sum;
    } pidAxisData_t;
    struct pid_config{
        uint8_t flag;
        pidf_t pid[PID_ITEM_COUNT];
        uint16_t yaw_lowpass_hz;     // Additional yaw filter when yaw axis too noisy
        uint16_t dterm_notch_hz; // Biquad dterm notch hz
        uint16_t dterm_notch_cutoff;// Biquad dterm notch low cutoff
        uint16_t dterm_lowpass_hz;   // Delta Filter in hz
        uint16_t dterm_lowpass2_hz;         // Extra PT1 Filter on D in hz
    };



    ZINO_CONFIG_DECLARE(struct pid_config, pid_config);

    extern pidRuntime_t pidRuntime;
    extern rollAndPitchTrims_t rollAndPitchTrims;
    extern struct pidProfile_s *currentPidProfile;
    extern pidAxisData_t pidData[XYZ_AXIS_COUNT];

    float calcHorizonLevelStrength(void);
    void attitude_controler(void);
    void attiude_controler_init(void);
    void pidUpdateAntiGravityThrottleFilter(float throttle);
    void save_pid_params(void);
    void pid_params_reload(void);
    void pid_params_reset(void);


#ifdef __cplusplus
}
#endif

#endif
